#include"gnss_imu_fusion/wrapper/gnss_imu_fusion_wrapper.h"
#include "nav_msgs/Path.h"
#include "nav_msgs/Odometry.h"

GnssImuFusionWrapper::GnssImuFusionWrapper(ros::NodeHandle &nh){
    fusion = std::make_shared<GNSSImuFusion>();
    // . 订阅话题
    std::string topic_name;
    nh.param<std::string>("imu_sub_topic",topic_name,"/imu");
    imu_sub = nh.subscribe(topic_name,10,&GnssImuFusionWrapper::imu_cb,this);
    nh.param<std::string>("gnss_sub_topic",topic_name,"/gnss"); 
    gnss_sub = nh.subscribe(topic_name,10,&GnssImuFusionWrapper::normal_gnss,this);
    // . 模式选择
    nh.param<int>("hz_mode",hz_mode,0);
    // . 发布话题
    path_pub = nh.advertise<nav_msgs::Path>("path",100);
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom",100);
    
}
GnssImuFusionWrapper::~GnssImuFusionWrapper(){

}
void GnssImuFusionWrapper::imu_cb(const sensor_msgs::ImuPtr &msg){

    SlamCraft::IMU imu;
    imu.acc = {msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z};
    imu.gro = {msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z};
    imu.stamp = msg->header.stamp.toSec();
    auto report = fusion->add(imu);
    if(report.is_predict&&hz_mode ==1){

        pub_msgs(report.predict_state,report.predict_time);
    }
    if(report.is_obs_update&&hz_mode==0){
        pub_msgs(report.obs_update_state,report.obs_update_time);
    }

}
void GnssImuFusionWrapper::normal_gnss(const sensor_msgs::NavSatFixPtr&msg){

    SlamCraft::GNSSDataType gnss;
    gnss.altitude = msg->altitude;
    gnss.longitude = msg->longitude;
    gnss.latitude = msg->latitude;
    gnss.stamp = msg->header.stamp.toSec();
    Obs obs;
    obs.gnss = gnss;
    obs.stamp = gnss.stamp;
    auto report = fusion->add(obs);
    if(report.is_obs_update&&hz_mode==0){

        pub_msgs(report.obs_update_state,report.obs_update_time);
    }

}

void GnssImuFusionWrapper::pub_msgs(SlamCraft::ESKF::State_18 state,double time){

    static nav_msgs::Path path;
    path.header.frame_id = "map";
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = state.position.x();
    pose.pose.position.y = state.position.y();
    pose.pose.position.z = state.position.z();
    path.poses.push_back(pose);
    path_pub.publish(path);

    auto q = state.rotation;

    nav_msgs::Odometry odom_message;
    odom_message.header.stamp = ros::Time().fromSec(time);
    odom_message.header.frame_id = "map";
    odom_message.child_frame_id = "body";
    odom_message.pose.pose.position.x = state.position.x();
    odom_message.pose.pose.position.y = state.position.y();
    odom_message.pose.pose.position.z = state.position.z();
    odom_message.pose.pose.orientation.w = q.w();
    odom_message.pose.pose.orientation.x = q.x();
    odom_message.pose.pose.orientation.y = q.y();
    odom_message.pose.pose.orientation.z = q.z();

    odom_pub.publish(odom_message);


}
